#include "tim.h"
#include "usart.h"
#include "bluetooth.h"
#include "spi.h"
#include "ht32f5xxxx_pdma.h"
#include "bldc_foc.h"
#include "AS5047P.h"
#include "system_ht32f5xxxx_01.h"
#define TIM_PRESCALE_VALUE(x)  (x - 1)
#define TIM_RELOAD_VALUE_US(x,prescale) (((48 / prescale) * x) - 1)
#define TIM_RELOAD_VALUE_MS(x,prescale) (((48000 / prescale) * x) -1)


#define SysTick_Prescale     8
#define __CK_AHB             48000000
#define SysTick_RELOAD_MS(x) (x * ((__CK_AHB / SysTick_Prescale) / 1000))
#define SysTick_RELOAD_US(x) (x * ((__CK_AHB / SysTick_Prescale) / 1000000))
#define SysTick_MAX_MS       (16777216 / __CK_AHB * SisTick_Prescale * 1000)

extern BLDC_Device BLDC_Swing;
extern BLDC_Device BLDC_Elbow;
extern Push_motor push_motor;
extern BLDC_RequestStatusTypeDef BLDC_RequestStatus;
extern void PassiveMode_Handler(BLDC_Device *);
void updateBluetoothDataPackage(Bluetooth_DataFrame *datapackage);
uint16_t  spi_rec  = 1;
uint16_t timer_call;
extern SPI_Device AS5047P;
unsigned int systick;
void _TIM_Base_Init(void)
{
    CKCU_PeripClockConfig_TypeDef CKCU_ConfigUnion;
    CKCU_ConfigUnion.Bit.AFIO  = 1;
    CKCU_ConfigUnion.Bit.GPTM0 = 1;

    CKCU_PeripClockConfig(CKCU_ConfigUnion,ENABLE);


    TM_TimeBaseInitTypeDef TIM_TimeBaseConfigStruct;
    TIM_TimeBaseConfigStruct.CounterMode       = TM_CNT_MODE_UP;
    TIM_TimeBaseConfigStruct.Prescaler         = TIM_PRESCALE_VALUE(480);
    TIM_TimeBaseConfigStruct.CounterReload     = TIM_RELOAD_VALUE_MS(25,480);
    TIM_TimeBaseConfigStruct.RepetitionCounter = 0;
    TIM_TimeBaseConfigStruct.PSCReloadTime     = TM_PSC_RLD_UPDATE;

    TM_TimeBaseInit(HT_GPTM0,&TIM_TimeBaseConfigStruct);

    TM_ClearFlag(HT_GPTM0,TM_INT_UEV);
    NVIC_EnableIRQ(GPTM0_IRQn);
    NVIC_SetPriority(GPTM0_IRQn, 2);
    TM_IntConfig(HT_GPTM0, TM_FLAG_UEV, ENABLE);

    TM_Cmd(HT_GPTM0, ENABLE);

    //SysTick Init
    SysTick->LOAD = SysTick_RELOAD_MS(1000);
    SysTick->CTRL = SysTick_CTRL_ENABLE_Msk | SysTick_CTRL_TICKINT_Msk;
}

void GPTM0_IRQHandler(void)
{
    static int led_stat = 0;
    static int BLDC_InquireID = 0;
    static int BluetoothFlag = 4;
    ++timer_call;
    switch (BLDC_RequestStatus.BLDC_Elbow || BLDC_RequestStatus.BLDC_SWing)
    {
        case 0:BLDC_ParamInquire = BLDC_GetParam; break;
        default:break;
    }
    switch (BLDC_InquireID)
    {
        case 0:BLDC_ParamInquire(&BLDC_Elbow);BLDC_InquireID = 1; BLDC_RequestStatus.BLDC_Elbow = 0; break;
        case 1:BLDC_ParamInquire(&BLDC_Swing);BLDC_InquireID = 0; BLDC_RequestStatus.BLDC_SWing = 0; break;
        default:break;
    }
    if(timer_call > BluetoothFlag)
    {
        updateBluetoothDataPackage(&BluetoothData);
        // PDMA_SwTrigCmd(PDMA_CH5, ENABLE);
	    HT_PDMA->PDMACH5.SADR = (unsigned int)(&BluetoothData);
        HT_PDMA->PDMACH5.TSR  = ((0x0B<<16) | 0x01);
        PDMA_EnaCmd(PDMA_CH5, ENABLE);
        BluetoothFlag += 4;
    }
    if(timer_call > 39){
        if(led_stat){
            GPIO_ClearOutBits(HT_GPIOC, GPIO_PIN_14);
			led_stat = 0;
        }
        else{
            GPIO_SetOutBits(HT_GPIOC, GPIO_PIN_14);
			led_stat = 1;
        }

        timer_call = 0;
        BluetoothFlag = 4;
    }

    // updateBluetoothDataPackage(&BluetoothData);
    // // PDMA_SwTrigCmd(PDMA_CH5, ENABLE);
	// HT_PDMA->PDMACH5.SADR = (unsigned int)(&BluetoothData);
    // HT_PDMA->PDMACH5.TSR  = ((0x0B<<16) | 0x01);
    // PDMA_EnaCmd(PDMA_CH5, ENABLE);
    TM_ClearFlag(HT_GPTM0, TM_FLAG_UEV);
}

void updateBluetoothDataPackage(Bluetooth_DataFrame *datapackage)
{
    uint16_t * bit16_bt_pointer, * bit16_motor_pointer;
    /*Update elbow*/
    bit16_bt_pointer = (uint16_t *)(&(datapackage->ElbowAngleL));
    bit16_motor_pointer = (uint16_t *)(&(BLDC_Elbow.position));
    *bit16_bt_pointer = *bit16_motor_pointer;
    /*Update swing*/
    bit16_bt_pointer = (uint16_t *)(&(datapackage->SwingAngleL));
    bit16_motor_pointer = (uint16_t *)(&(BLDC_Swing.position));
    *bit16_bt_pointer = *bit16_motor_pointer;
    /*Update lift*/
    bit16_bt_pointer = (uint16_t *)(&(datapackage->LiftAngleL));
    bit16_motor_pointer = (uint16_t *)(&(push_motor.current_angle));
    *bit16_bt_pointer = *bit16_motor_pointer - push_motor.offset;
    // *bit16_bt_pointer = 32768;

}

//
void SysTick_Handler(void)
{
    if((SysTick->CTRL & (0x01 << 16)) && (SysTick->CTRL & 0x01))
    {
        // switch(systick)
        // {
        //     case 0:
        //     case 1: ++systick;break;
        //     default: systick = 0;break;
        // }
        ++systick;
    }
    SysTick->CTRL &= ~(SysTick_CTRL_ENABLE_Msk);
    SysTick->VAL = 0;
}
